﻿using System;

namespace GeoFits
{    
    /// <summary>
    /// 抗差最小二乘设置
    /// </summary>
    public class RLSConfig
    {
        /// <summary>
        /// 循环次数
        /// 注意：表示在getP0之后再循环的最多次数
        /// </summary>
        public int Loops
        { get; set; }
        /// <summary>
        /// 默认为Complex模式
        /// </summary>
        public RLSConfig()
        {
            Loops = int.MaxValue;
            setWeightDelegate();
        }
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="loops">循环次数</param>
        public RLSConfig(int loops)
        {
            Loops = loops;
            setWeightDelegate();
        }

        private void setWeightDelegate()
        {
            getP0 = Adjustment.getWeightL1;
            getWeight = Adjustment.getWeightIGG;
        }

        /// <summary>
        /// P0的选权函数，建议使用一次范数最小法
        /// </summary>
        public Adjustment.getWeightDelegate getP0;
        /// <summary>
        /// 抗差最小二乘法选权方案
        /// </summary>
        public Adjustment.getWeightDelegate getWeight;
        /// <summary>
        /// P0的选权方案的条件
        /// </summary>
        public object[] P0Conditions
        { get; private set; }
        /// <summary>
        /// 设置P0的选权方案的条件
        /// </summary>
        /// <param name="conditions">要设置的条件</param>
        public void setP0Conditions(params object[] conditions)
        {
            P0Conditions = conditions;
        }
        /// <summary>
        /// P0的选权方案的条件
        /// </summary>
        public object[] WeightConditions
        { get; private set; }
        /// <summary>
        /// 设置P0的选权方案的条件
        /// </summary>
        /// <param name="conditions">要设置的条件</param>
        public void setWeightConditions(params object[] conditions)
        {
            WeightConditions = conditions;
        }        
    }
    /// <summary>
    /// 平差方法
    /// </summary>
    public class Adjustment
    {
        /// <summary>
        /// 间接平差解算（Indirect Observations）
        /// 具体解算方法可以参照工业测量拟合（王解先、季凯敏2.2.2 P15-16）
        /// </summary>
        /// <param name="A">系数矩阵A</param>
        /// <param name="L">常数项矩阵L</param>
        /// <param name="P">权阵P</param>
        /// <returns>未知数（改正数）矩阵X</returns>
        public static Matrix SolveIO(Matrix A, Matrix L, Matrix P = null)
        {
            if (Matrix.ReferenceEquals(P,null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            Matrix tempM = Matrix.Transpose(A) * P;
            Matrix N = tempM * A;
            Matrix C = tempM * L;
            return Matrix.Inverse(N) * C;
        }        
        /// <summary>
        /// 间接平差解算（Indirect Observations）
        /// 方程为 V=AX-L
        /// 具体解算方法可以参照参照误差理论与平差基础（武汉大学测绘学院测量平差教学组7.1 P107）
        /// 协因数的定义可以参照误差理论与平差基础（武汉大学测绘学院测量平差教学组3.4 P46-47）
        /// 协因数阵的计算可以参照误差理论与平差基础（武汉大学测绘学院测量平差教学组7.3 P122-123）
        /// </summary>
        /// <param name="A">系数矩阵A</param>
        /// <param name="L">常数项矩阵L</param>   
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qxx">未知数X的协因数矩阵</param>
        /// <param name="Qvv">每个方程改正数V的协因数矩阵</param>
        /// <param name="V">参差向量</param>
        /// <param name="P">权阵P</param>
        /// <returns>未知数（改正数）矩阵X</returns>
        public static Matrix SolveIO(Matrix A, Matrix L, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, Matrix P = null)
        {
            if (Matrix.ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            Matrix tempM = Matrix.Transpose(A) * P;
            Matrix N = tempM * A;
            Matrix C = tempM * L;
            Matrix NInverse = Matrix.Inverse(N);
            Qxx = NInverse;
            int rows = P.NoRows;
            Matrix Q = new Matrix(Matrix.Identity(rows));
            //在用于抗差最小二乘法或其他特殊情况下，P不为满秩矩阵，不能直接用P的逆来求Q
            for (int i = 0; i < rows; i++)
            {
                if (P[i, i] != 0)
                    Q[i, i] = 1 / P[i, i];
                else
                    Q[i, i] = double.MaxValue;
            }
            Qvv = Q - A * NInverse * Matrix.Transpose(A);
            Matrix X = NInverse * C;
            V = A * X - L;
            Matrix VTPV = Matrix.Transpose(V) * P * V;
            //类似于上面求Q，当P不为满秩矩阵时，n的算法要有所不同
            int n = Matrix.Rank(P);
            sigma0 = Math.Sqrt(VTPV[0, 0] / (n - A.NoCols));
            return X;
        }
        
        /// <summary>
        /// 带有参数的间接平差(Indirect Observations With Constraints)
        /// 方程为: V=AX-L, CX+W=0
        /// 关于此方法的使用：建议在条件允许的情况下尽量使用间接平差，因为此方法计算量明显比间接平差大很多。
        /// 并且此方法并没有相应的选权迭代法进行抗差最小二乘法的相关函数。
        /// </summary>
        /// <returns>未知数（改正数）矩阵X</returns>
        public static Matrix SolveIOWC(Matrix A, Matrix L, Matrix C, Matrix W, Matrix P = null)
        {
            if (Matrix.ReferenceEquals(P,null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            Matrix Naa=Matrix.Transpose(A)*P*A;
            Matrix Wal=Matrix.Transpose(A)*P*L;
            Matrix NaaInverse=Matrix.Inverse(Naa);
            Matrix Ncc=C*NaaInverse*Matrix.Transpose(C);
            Matrix NccInverse=Matrix.Inverse(Ncc);
            Matrix tempM=NaaInverse*Matrix.Transpose(C)*NccInverse;
            return (NaaInverse-tempM*C*NaaInverse)*Wal-tempM*W;
        }
        /// <summary>
        /// 带有参数的间接平差(Indirect Observations With Constraints)
        /// 方程为: V=AX-L, CX+W=0
        /// </summary>
        /// <returns>未知数（改正数）矩阵X</returns>
        public static Matrix SolveIOWC(Matrix A, Matrix L, Matrix C, Matrix W, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, Matrix P = null)
        {
            if (Matrix.ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            //当P不为满秩矩阵的时候NaaInverse会有问题
            Matrix Naa = Matrix.Transpose(A) * P * A;
            Matrix Wal = Matrix.Transpose(A) * P * L;
            Matrix NaaInverse = Matrix.Inverse(Naa);
            Matrix Ncc = C * NaaInverse * Matrix.Transpose(C);
            Matrix NccInverse = Matrix.Inverse(Ncc);
            Matrix tempM = NaaInverse * Matrix.Transpose(C) * NccInverse;
            Qxx = NaaInverse - tempM * C * NaaInverse;
            Qvv = Matrix.Inverse(P) - A * Qxx * Matrix.Transpose(A);
            Matrix X = Qxx * Wal - tempM * W;
            V = A * X - L;
            Matrix VTPV = Matrix.Transpose(V) * P * V;
            double r = A.NoRows - A.NoCols + C.NoRows;
            sigma0 = Math.Sqrt(VTPV[0, 0] / r);
            return X;
        }

        /// <summary>
        /// 抗差最小二乘法（Robustified Least Squares Method）
        /// 调用方法：
        /// 1、如果直接是线性给的模型，那么直接调用此方法就进行抗差最小二乘法
        /// 2、如果本身函数模型是线性化过的，那么要循环调用才可。
        /// 当然也可以只进行两次迭代，第一次用一次范数法，抗差性比较好，然后在此基础上再进行一次抗差的平差。
        /// </summary>
        /// <param name="A">参数矩阵</param>
        /// <param name="L">常数矩阵</param>
        /// <param name="getWeight">选权函数</param>
        /// <param name="PA">后验权：注意此参数可以传入也可以传出</param>
        /// <param name="P">先验权</param>
        /// <param name="thisBreak">跳出循环条件</param>
        /// <param name="conditions">使用的选权函数的限制条件</param>
        /// <returns>最终的估计值</returns>
        public static Matrix SolveRLS(Matrix A, Matrix L, getWeightDelegate getWeight, ref Matrix PA, Matrix P = null, FitPattern.LinearizedModuleBreakDelegate thisBreak = null, params object[] conditions)
        {
            //此方法有问题，因为即使给了较好的初始值，第一次平差仍然是错误
            if(ReferenceEquals(thisBreak,null))
            {
                thisBreak = FitPattern.defaultBreak;
            }
            if (ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            double sigma0;
            Matrix Qxx, Qvv, V, X1, X0;
            if (ReferenceEquals(PA, null))
            {
                //首先如果没有后验权，进行一次最小二乘平差，并设置后验权
                SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, P);
                PA = getWeight(sigma0, V, P, conditions);
            }
            //设置X0
            X0 = SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, PA);
            PA = getWeight(sigma0, V, P, conditions);
            while (true)
            {
                X1 = SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, PA);
                PA = getWeight(sigma0, V, P, conditions);
                if (thisBreak(X0, X1))
                { break; }
                X0 = X1;
            }
            return X1;
        }
        /// <summary>
        /// 抗差最小二乘法（Robustified Least Squares Method）
        /// 调用方法：
        /// 因为在很多情况下都是非线性的模型线性化的结果，首次进行初始值不会太准确，所以要进行循环。
        /// 当然也可以只进行两次迭代，第一次用一次范数法，抗差性比较好，然后在此基础上再进行一次抗差的平差。
        /// </summary>
        /// <param name="A">参数矩阵</param>
        /// <param name="L">常数矩阵</param>
        /// <param name="getWeight">选权函数</param>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qxx">未知数X的协方差阵</param>
        /// <param name="Qvv">改正数V的协方差阵</param>
        /// <param name="V">改正数向量</param>
        /// <param name="PA">后验权：此参数可以传入也可以传出</param>
        /// <param name="P">先验权</param>
        /// <param name="thisBreak">跳出循环条件</param>
        /// <param name="conditions">使用的选权函数的限制条件</param>
        /// <returns>最终的估计值</returns>
        public static Matrix SolveRLS(Matrix A, Matrix L, getWeightDelegate getWeight, out double sigma0, out Matrix Qxx, out Matrix Qvv, out Matrix V, ref Matrix PA, Matrix P = null, FitPattern.LinearizedModuleBreakDelegate thisBreak = null, params object[] conditions)
        {
            if (ReferenceEquals(thisBreak, null))
            {
                thisBreak = FitPattern.defaultBreak;
            }
            if (ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(A.NoRows));
            }
            Matrix X1, X0;
            if (ReferenceEquals(PA, null))
            {
                //首先如果没有后验权，进行一次最小二乘平差，并设置后验权
                SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, P);
                PA = getWeight(sigma0, V, P, conditions);
            }
            //设置X0
            X0 = SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, PA);
            PA = getWeight(sigma0, V, P, conditions);


            while (true)
            {
                //PA为后验证权                
                X1 = SolveIO(A, L, out sigma0, out Qxx, out Qvv, out V, PA);
                PA = getWeight(sigma0, V, P, conditions);
                if (thisBreak(X0, X1))
                { break; }
                X0 = X1;
            }
            return X1;
        }
        
        /// <summary>
        /// 选权迭代法选权函数的代理
        /// </summary>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="Qvv">改正数V的协因数阵</param>
        /// <param name="P">先验权矩阵</param>
        /// <param name="conditions">其他限制条件</param>
        /// <returns>后验权矩阵</returns>
        public delegate Matrix getWeightDelegate(double sigma0, Matrix Qvv, Matrix P, params object[] conditions);

        /// <summary>
        /// IGG选权方案
        /// 在写选权方案函数的时候应当注意
        /// Qvv的意义：第一个对角元上的数是：(sigmai)的平方/(sigma0)的平方
        /// </summary>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="V">改正数V的协因数阵</param>
        /// <param name="P">先验权矩阵</param>
        /// <param name="conditions">
        /// 第一个参数：IGG_DROP_WEIGHT_POINT，类型：double
        /// 第二个参数：IGG_OUT_POINT，类型：double</param>
        /// <returns>后验权矩阵</returns>
        public static Matrix getWeightIGG(double sigma0, Matrix V, Matrix P, params object[] conditions)
        {
            double IGG_DROP_WEIGHT_POINT, IGG_OUT_POINT;
            if(ReferenceEquals(conditions, null))
            {
                IGG_DROP_WEIGHT_POINT = 1.5;
                IGG_OUT_POINT = 2.5;
            }else
            {
                if (conditions.Length < 1)
                {
                    IGG_DROP_WEIGHT_POINT = 1.5;
                    IGG_OUT_POINT = 2.5;
                }
                else if (conditions.Length == 1)
                {
                    IGG_DROP_WEIGHT_POINT = Convert.ToDouble(conditions[0]);
                    IGG_OUT_POINT = 2.5;
                }
                else
                {
                    IGG_DROP_WEIGHT_POINT = Convert.ToDouble(conditions[0]);
                    IGG_OUT_POINT = Convert.ToDouble(conditions[1]);
                }
            }            
            
            if (ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(V.NoRows));
            }
            int n = V.NoRows;
            Matrix PA = new Matrix(Matrix.Identity(n));
            for(int i=0; i < n; i++)
            {
                if(Math.Abs(V[i, 0]) < IGG_DROP_WEIGHT_POINT * sigma0)
                {
                    PA[i, i] = P[i, i];
                }
                else if(Math.Abs(V[i, 0]) < IGG_OUT_POINT * sigma0)
                {
                    //drop weight
                    PA[i, i] = IGG_DROP_WEIGHT_POINT * sigma0 * P[i, i] / Math.Abs(V[i, 0]);
                }
                else
                { PA[i, i] = 0; }
            }
            return PA;
        }

        /// <summary>
        /// L1选权方案(一次范数最小，绝对值和最小，中位数)
        /// </summary>
        /// <param name="sigma0">单位权中误差</param>
        /// <param name="V">改正数V的协因数阵</param>
        /// <param name="P">先验权矩阵</param>
        /// <param name="conditions">
        /// 第一个参数：L1_C_PARAM，类型：double，意义：一个比较小的常数，默认为0.001</param>
        /// <returns>后验权矩阵</returns>
        public static Matrix getWeightL1(double sigma0, Matrix V, Matrix P, params object[] conditions)
        {
            double L1_C_PARAM;
            if (ReferenceEquals(conditions, null))            
                L1_C_PARAM = 0.001;
            else
                if (conditions.Length < 1)                
                    L1_C_PARAM = 0.001;                
                else                
                    L1_C_PARAM = Convert.ToDouble(conditions[0]);
            if (ReferenceEquals(P, null))
            {
                P = new Matrix(Matrix.Identity(V.NoRows));
            }
            int n = V.NoRows;
            Matrix PA = new Matrix(Matrix.Identity(n));
            for (int i = 0; i < n; i++)
            {
                PA[i, i] = P[i, i] / (Math.Abs(V[i, 0]) + L1_C_PARAM);
                //if(Math.Abs(V[i, 0]) == 0)
                //{
                //    PA[i, i] = double.MaxValue;
                //}else
                //{
                //    PA[i, i] = P[i, i] / Math.Abs(V[i, 0]);
                //}                
            }           

            return StandardizationPA(PA, P);
        }
        /// <summary>
        /// 由于权阵的改变，导致了一个非常严重的问题
        /// 权会很大，导致VTPV很大，就导致了单位权中误差很大，因此，我们对权阵进行了单位化
        /// </summary>
        /// <param name="PA"></param>
        /// <param name="P"></param>
        /// <returns></returns>
        private static Matrix StandardizationPA(Matrix PA, Matrix P)
        {
            int n = PA.NoRows;
            double addP = 0, addPa = 0, para = 0;
            for (int i = 0; i < n; i++)
            {
                addP += P[i, i] * P[i, i];
                addPa += PA[i, i] * PA[i, i];
            }
            para = Math.Sqrt(addPa) / Math.Sqrt(addP);
            for (int i = 0; i < n; i++)
            {
                PA[i, i] = PA[i, i] / para;
            }
            return PA;
        }
    }
}
